#!/usr/bin/env python

import rospy

from geometry_msgs.msg import Twist
from std_msgs.msg import String
import sys, select, termios, tty




class SubNPub():
    def __init__(self):
        self.settings = termios.tcgetattr(sys.stdin)
        rospy.init_node('turtlebot_teleop')
        self.count = 0
        self.pub_ = rospy.Publisher('~cmd_vel', Twist, queue_size=5)
        self.sub = rospy.Subscriber('kws_data', String, self.kwsCallback)
        self.moveBindings = {
                    'i':(1,0),
                    'o':(1,-1),
                    'j':(0,1),
                    'l':(0,-1),
                    'u':(1,1),
                    ',':(-1,0),
                    '.':(-1,1),
                    'm':(-1,-1),
                    }

        self.SmoveBindings = {
            'move ':(1,0),
            # 'o':(1,-1),
            'left ':(0,1),
            'right ':(0,-1),
            # 'u':(1,1),
            'back ':(-1,0),
            # '.':(-1,1),
            # 'm':(-1,-1),
            }

        self.speedBindings={
            'q':(1.1,1.1),
            'z':(.9,.9),
            'w':(1.1,1),
            'x':(.9,1),
            'e':(1,1.1),
            'c':(1,.9),
            }
        self.speed = .2
        self.turn = 1
        self.x = 0
        self.th = 0
        self.status = 0
        self.acc = 0.1
        self.target_speed = 0
        self.target_turn = 0
        self.control_speed = 0
        self.control_turn = 0
        self.tarl = 0
        self.tara = 0

        self.soundvec = [0,0,0,0]


    def kwsCallback(self, data):
        try:
            while len(data.data) > 0:
                key = data.data
                
                if key == "move":
                    if self.soundvec[3]>0:
                        self.soundvec[3] -=1
                    else:
                        self.soundvec[0] = min(2,self.soundvec[0]+1)
                elif key == "back":
                    if self.soundvec[0]>0:
                        self.soundvec[0] -=1
                    else:
                        self.soundvec[3] = min(2,self.soundvec[3]+1)
                elif key == "left":
                    if self.soundvec[2]>0:
                        self.soundvec[2] -=1
                    else:
                        self.soundvec[1] = min(2,self.soundvec[1]+1)
                elif key == "right":
                    if self.soundvec[1]>0:
                        self.soundvec[1] -=1
                    else:
                        self.soundvec[2] = min(2,self.soundvec[2]+1)

                self.x = self.soundvec[0]-self.soundvec[3]
                self.th = self.soundvec[1]-self.soundvec[2]

                self.target_speed = self.speed * self.x
                self.target_turn = self.turn * self.th

                # if self.target_speed > self.control_speed:
                #     self.control_speed = min( self.target_speed, self.control_speed + 0.02 )
                # elif self.target_speed < self.control_speed:
                #     self.control_speed = max( self.target_speed, self.control_speed - 0.02 )
                # else:
                #     self.control_speed = self.target_speed

                # if self.target_turn > self.control_turn:
                #     self.control_turn = min( self.target_turn, self.control_turn + 0.1 )
                # elif self.target_turn < self.control_turn:
                #     self.control_turn = max( self.target_turn, self.control_turn - 0.1 )
                # else:
                #     self.control_turn = self.target_turn

                # twist = Twist()
                # twist.linear.x = self.control_speed; twist.linear.y = 0; twist.linear.z = 0
                # twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = self.control_turn
                # self.pub.publish(twist)

                print("loop: {0}".format(self.count))
                print("sound msg: {0}".format(key))
                print("target: vx: {0}, wz: {1}".format(self.target_speed, self.target_turn))
                # print("publihsed: vx: {0}, wz: {1}".format(twist.linear.x, twist.angular.z))
            # rospy.spin()
        except Exception as e:
            print(e)

        finally:
            twist = Twist()
            twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
            self.pub.publish(twist)

        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)


    def cmdnow(self): # 更新cmd数值
        if self.target_speed > self.control_speed:
            self.control_speed = min( self.target_speed, self.control_speed + 0.02 )
        elif self.target_speed < self.control_speed:
            self.control_speed = max( self.target_speed, self.control_speed - 0.02 )
        else:
            self.control_speed = self.target_speed

        if self.target_turn > self.control_turn:
            self.control_turn = min( self.target_turn, self.control_turn + 0.1 )
        elif self.target_turn < self.control_turn:
            self.control_turn = max( self.target_turn, self.control_turn - 0.1 )
        else:
            self.control_turn = self.target_turn
        
        # rospy.loginfo(self.soundvec)
        # rospy.loginfo("tarL:%.2f\ttarA:%.2f",self.tarl,self.tara)




if __name__ == '__main__':
    n = SubNPub()
    rate = rospy.Rate(10)
    count = 0
    twist = Twist()
    while not rospy.is_shutdown():
        # lz の P control 版本 ↓
        # twist.linear.x+=0.5*(n.tarl-twist.linear.x)
        # twist.angular.z+=0.05*(n.tara-twist.angular.z)

        # ordinary 版本 ↓
        n.cmdnow()
        twist.linear.x = n.control_speed; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = n.control_turn

        n.pub_.publish(twist)
        rospy.loginfo("liner:%.2f, ang:%.2f",twist.linear.x,twist.angular.z)
        count+=1
        rate.sleep()